
Left=0  #左转控制，范围：-1到1，值越大转向幅度越大
Right=0  #右转控制，范围：-1到1，值越大转向幅度越大
key_stab=False #稳定控制标志，True：启用自稳，False：禁用自稳
PIT_goal=0  #目标俯仰角度，单位：度，用于姿态调节
ROL_goal=0  #目标滚转角度，单位：度，用于姿态调节
YAW_goal=0  #目标偏航角度，单位：度，用于姿态调节
X_goal=0    #目标X位置，单位：毫米，用于位置控制
pit_max_ang=15  #俯仰轴最大角度限制，单位：度，防止机器狗过度前倾或后仰
rol_max_ang=20  #滚转轴最大角度限制，单位：度，防止机器狗过度侧倾

Ts=1       #周期，单位：秒。控制机器狗运动的基本时间单位，值越大运动越慢但更稳定，值越小运动越快但可能不稳定，建议值范围：0.5-2秒
faai=0.5   #占空比，控制信号的高电平时间与周期时间的比值。用于调节PWM信号，值越大舵机转动越快但精度可能降低，值越小转动越慢但更精确
Kp_V=0.2   #速度变化P环，比例系数。用于调节速度变化的响应速度，值越大响应越快但可能引起振荡，建议值范围：0.1-0.5

#=============一些中间或初始变量=============
deltaStep=0  #时间变量，用于控制周期
init_x=0;init_y=-106;init_z=0  #初始位置坐标，单位：毫米

#ges_x_1=0;ges_x_2=0;ges_x_3=0;ges_x_4=0  #四条腿的X轴位置，单位：毫米
#ges_y_1=init_y;ges_y_2=init_y;ges_y_3=init_y;ges_y_4=init_y  #四条腿的Y轴位置，单位：毫米
#ges_z_1=init_z;ges_z_2=init_z;ges_z_3=init_z;ges_z_4=init_z  #四条腿的Z轴位置，单位：毫米

PIT_S=0;ROL_S=0;YAW_S=0;X_S=0  #当前姿态值：俯仰、滚转、偏航、X位置

spd=0;spd_goal=0  #当前速度和目标速度，单位：米/秒
R_H=abs(init_y);H_goal=106  #当前高度和目标高度，单位：毫米
init_case=0  #初始化标志，0：未初始化，1：已初始化
gait_mode=0  #步态模式，0：TROT步态，2：平移步态